import json
import math

class Vector():
    def __init__(self,x=0,y=0,z=0):
        self.x = x
        self.y = y
        self.z = z

    def set_value(self,vec):
        self.x = vec.x
        self.y = vec.y
        self.z = vec.z

    # 求向量模长
    def modulus(self):
        return math.sqrt(self.x * self.x + self.y * self.y + self.z * self.z)



    def multiply_list(self,mul_list):
        if not len(mul_list) == 3:
            print('[Warn]:Length of mul_list should be 3.')
            return
        self.x *= mul_list[0] 
        self.y *= mul_list[1] 
        self.z *= mul_list[2] 

    def negate(self):
        return Vector(-self.x,-self.y,-self.z)

    def cosv(self,vec):
        dot = self.x * vec.x + self.y * vec.y + self.z * vec.z
        m1 = self.modulus()
        m2 = vec.modulus()
        return dot/m1/m2

    @staticmethod
    def minus(v,vp):
        x = v.x - vp.x
        y = v.y - vp.y
        z = v.z - vp.z
        return Vector(x,y,z)
    
    @staticmethod
    def add(v,vp):
        x = v.x + vp.x
        y = v.y + vp.y
        z = v.z + vp.z
        return Vector(x,y,z)

    def printv(self):
        print("\rVector: x: {0:.4f}, y: {1:.4f}, z: {2:.4f}".format(self.x,self.y,self.z),end='', flush=True)

    def __str__(self) -> str:
        return json.dumps([self.x, self.y, self.z])

class Quaternion():
    def __init__(self,w=1,x=0,y=0,z=0):
        self.w = w
        self.x = x
        self.y = y
        self.z = z

    def set_value_list(self,q_list):
        self.w = q_list[0]
        self.x = q_list[1]
        self.y = q_list[2]
        self.z = q_list[3]

    def set_value(self,q):
        self.w = q.w
        self.x = q.x
        self.y = q.y
        self.z = q.z

    def multiply(self,q):
        w = self.w * q.w - self.x * q.x - self.y * q.y - self.z * q.z
        x = self.w * q.x + self.x * q.w + self.y * q.z - self.z * q.y
        y = self.w * q.y + self.y * q.w + self.z * q.x - self.x * q.z
        z = self.w * q.z + self.z * q.w + self.x * q.y - self.y * q.x
        self.w = w
        self.x = x
        self.y = y
        self.z = z

    def switch_xy(self):
        x = self.x
        y = self.y
        self.y = x
        self.x = y

    def switch_xz(self):
        x = self.x
        z = self.z
        self.z = x
        self.x = z

    def multiply_list(self,mul_list):
        if not len(mul_list) == 4:
            print('[Warn]:Length of mul_list should be 4.')
            return
        self.w *= mul_list[0]
        self.x *= mul_list[1] 
        self.y *= mul_list[2] 
        self.z *= mul_list[3] 

    @staticmethod
    def mul(s,q):
        w = s.w * q.w - s.x * q.x - s.y * q.y - s.z * q.z
        x = s.w * q.x + s.x * q.w + s.y * q.z - s.z * q.y
        y = s.w * q.y + s.y * q.w + s.z * q.x - s.x * q.z
        z = s.w * q.z + s.z * q.w + s.x * q.y - s.y * q.x
        return Quaternion(w,x,y,z)
    
    @staticmethod
    def rotate(q,vq):
        w2 = q.w * q.w
        x2 = q.x * q.x
        y2 = q.y * q.y
        z2 = q.z * q.z
        wx = q.w * q.x
        wy = q.w * q.y
        wz = q.w * q.z
        xy = q.x * q.y
        yz = q.y * q.z
        xz = q.z * q.x
        x = (w2 + x2 - y2 - z2) * vq.x + 2 * (wz + xy) * vq.y + 2 * (xz - wy) * vq.z
        y = 2 * (xy - wz) * vq.x + (w2 - x2 + y2 - z2) * vq.y + 2 * (wx + yz) * vq.z
        z = 2 * (wy + xz) * vq.x + 2 * (yz - wx) * vq.y + (w2 - x2 - y2 + z2) * vq.z
        return Vector(x,y,z)

    # 共轭四元数
    def adjointize(self):
        self.x = -self.x
        self.y = -self.y
        self.z = -self.z

    def get_adjoint(self):
        return Quaternion(self.w,-self.x,-self.y,-self.z)
    
    def printq(self):
        print("\rQuaternion: w: {0:.4f}, x: {1:.4f}, y: {2:.4f}, z: {3:.4f}".format(self.w,self.x,self.y,self.z),end='', flush=True)